#include "boundingbox.h"

// Included files for OpenGL Rendering
#ifdef __APPLE__
#include <OpenGL/gl.h>
#include <OpenGL/glu.h>
#include <GLUT/glut.h>
#else
#include <GL/gl.h>
#include <GL/glu.h>
#include <GL/glut.h>
#endif

#include "cloth.h"
#include "argparser.h"
#include <fstream>
#include "vectors.h"

using namespace std;

// ================================================================================
// some helper drawing functions

void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4);
void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction);

// ================================================================================
// ================================================================================

Cloth::Cloth(ArgParser *_args) {
  args =_args;

  // open the file
  ifstream istr(args->cloth_file.c_str());
  //assert (istr != NULL);
  string token;

  // read in the simulation parameters
  istr >> token >> k_structural; assert (token == "k_structural");  // (units == N/m)  (N = kg*m/s^2)
  istr >> token >> k_shear; assert (token == "k_shear");
  istr >> token >> k_bend; assert (token == "k_bend");
  istr >> token >> damping; assert (token == "damping");
  // NOTE: correction factor == .1, means springs shouldn't stretch more than 10%
  //       correction factor == 100, means don't do any correction
  istr >> token >> provot_structural_correction; assert (token == "provot_structural_correction");
  istr >> token >> provot_shear_correction; assert (token == "provot_shear_correction");

  // the cloth dimensions
  istr >> token >> nx >> ny; // (units == meters)
  assert (token == "m");
  assert (nx >= 2 && ny >= 2);

  // the corners of the cloth
  Vec3f a,b,c,d;
  istr >> token >> a; assert (token == "p");
  istr >> token >> b; assert (token == "p");
  istr >> token >> c; assert (token == "p");
  istr >> token >> d; assert (token == "p");

  // fabric weight  (units == kg/m^2)
  // denim ~300 g/m^2
  // silk ~70 g/m^2
  float fabric_weight;
  istr >> token >> fabric_weight; assert (token == "fabric_weight");
  float area = AreaOfTriangle(a,b,c) + AreaOfTriangle(a,c,d);

  // create the particles
  particles = new ClothParticle[nx*ny];
  float mass = area*fabric_weight / float(nx*ny);
  for (int i = 0; i < nx; i++) {
    float x = i/float(nx-1);
    Vec3f ab = (1-x)*a + x*b;
    Vec3f dc = (1-x)*d + x*c;
    for (int j = 0; j < ny; j++) {
      float y = j/float(ny-1);
      ClothParticle &p = getParticle(i,j);
      Vec3f abdc = (1-y)*ab + y*dc;
      p.setOriginalPosition(abdc);
      p.setPosition(abdc);
      p.setVelocity(Vec3f(0,0,0));
      p.setMass(mass);
      p.setFixed(false);
    }
  }

  // the fixed particles
  while (istr >> token) {
    assert (token == "f");
    int i,j;
    float x,y,z;
    istr >> i >> j >> x >> y >> z;
    ClothParticle &p = getParticle(i,j);
    p.setPosition(Vec3f(x,y,z));
    p.setFixed(true);
  }
  
  //Set spring relationships
  //
	for(int ii=0; ii<nx; ii++)
	{
		for(int jj=0; jj<ny; jj++)
		{
			//cout<<ii<<" "<<jj<<":"<<endl;
			if(ii+1<nx) //struct -
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj);
				p.struc.push_back(make_pair(ii+1,jj));//p2);
				p2.struc.push_back(make_pair(ii,jj));//p);
				//cout<<" "<<p2.getPosition()<<endl;
			}
			if(jj+1<ny) //struct |
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii,jj+1);
				p.struc.push_back(make_pair(ii,jj+1));//p2);
				p2.struc.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
			if( (jj+1<ny) && (ii+1<nx) ) //shear 
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj+1);
				p.shear.push_back(make_pair(ii+1,jj+1));
				p2.shear.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			if( (jj-1>=0) && (ii+1<nx) ) //shear /
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+1,jj-1);
				p.shear.push_back(make_pair(ii+1,jj-1));
				p2.shear.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
			if(jj+2<ny) //bend |
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii,jj+2);
				p.bend.push_back(make_pair(ii,jj+2));
				p2.bend.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			if(ii+2<nx) //bend -
			{
				ClothParticle &p = getParticle(ii,jj);
				ClothParticle &p2 = getParticle(ii+2,jj);
				p.bend.push_back(make_pair(ii+2,jj));
				p2.bend.push_back(make_pair(ii,jj));
				//cout<<" "<<p2.getPosition()<<endl;
			}
			
		}
	}
  
  
  computeBoundingBox();
}

// ================================================================================

void Cloth::computeBoundingBox() {
  box = BoundingBox(getParticle(0,0).getPosition());
  for (int i = 0; i < nx; i++) {
    for (int j = 0; j < ny; j++) {
      box.Extend(getParticle(i,j).getPosition());
    }
  }
}

// ================================================================================
// ================================================================================
// ================================================================================

float distance(Vec3f a,Vec3f b)
{
	return 0;//return sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
}

void Cloth::Paint() const {

  // =====================================================================================
  // render the bounding box
  // =====================================================================================
  if (args->bounding_box) 
    box.Paint();

  // =====================================================================================
  // render the particles
  // =====================================================================================

  if (args->particles) {
    glDisable(GL_LIGHTING);
    glPointSize(3);
    glBegin(GL_POINTS);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
	const ClothParticle &p = getParticle(i,j);
	const Vec3f &pos = p.getPosition();
	bool fixed = p.isFixed();
	if (fixed) 
	  glColor3f(0,1,0);
	else
	  glColor3f(0,0,0);	
	glVertex3f(pos.x(),pos.y(),pos.z());
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the velocity at each particle
  // =====================================================================================

  if (args->velocity) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(1,0,0);
    for (int i = 0; i < nx; i++) {
      for (int j = 0; j < ny; j++) {
        const ClothParticle &p = getParticle(i,j);
        const Vec3f &pos = p.getPosition();
        const Vec3f &vel = p.getVelocity();
        Vec3f v = pos + vel;
        glVertex3f(pos.x(),pos.y(),pos.z());
        glVertex3f(v.x(),v.y(),v.z());
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the force at each particle
  // =====================================================================================

  if (args->force) {
    glDisable(GL_LIGHTING);
    glLineWidth(2);
    glBegin(GL_LINES);
    glColor3f(0,0,1);
    for (int i = 0; i < nx; i++) 
	{
      for (int j = 0; j < ny; j++) 
	  {
        const ClothParticle &p = getParticle(i,j);
		const Vec3f &pos = p.getPosition();
		Vec3f struc_force=Vec3f(0,0,0);
		Vec3f shear_force=Vec3f(0,0,0);
		Vec3f bend_force=Vec3f(0,0,0);
		for(int k=0; k<p.struc.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.struc[k].first,p.struc[k].second);
			//calculate struct spring force on p from p2
			
			/*Vec3f s_pos= (p2.getPosition()-p.getPosition()) - (p2.getOriginalPosition()-p.getOriginalPosition());   
			s_pos=s_pos*(k_structural);
			struc_force=s_pos;//struc_force+s_pos;*/
			
			//cout<<s_pos<<endl;
			
			Vec3f a=p2.getPosition();
			Vec3f b=p.getPosition();
			Vec3f c=p2.getOriginalPosition();
			Vec3f d=p.getOriginalPosition();
			//calculate struct spring force on p from p2
			//Vec3f s_pos= (p2.getPosition()-p.getPosition()) - (p2.getOriginalPosition()-p.getOriginalPosition());  
			Vec3f s_pos= Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) - Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			
			s_pos=s_pos;//*(k_structural);
			
			
			Vec3f pos2=p2.getPosition();
			glVertex3f(pos.x(),pos.y(),pos.z());
			glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());//pos2.x(),pos2.y(),pos2.z());
			//glVertex3f(struc_force.x(),struc_force.y(),struc_force.z());
			
		}
		for(int k=0; k<p.shear.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.shear[k].first,p.shear[k].second);
			//calculate shear spring force on p from p2
			/*Vec3f s_pos= (p2.getPosition()-p.getPosition()) - (p2.getOriginalPosition()-p.getOriginalPosition());   
			s_pos=s_pos*(k_shear);
			shear_force=s_pos;//shear_force+s_pos;*/
			
			Vec3f a=p2.getPosition();
			Vec3f b=p.getPosition();
			Vec3f c=p2.getOriginalPosition();
			Vec3f d=p.getOriginalPosition();
			//calculate struct spring force on p from p2
			//Vec3f s_pos= (p2.getPosition()-p.getPosition()) - (p2.getOriginalPosition()-p.getOriginalPosition());  
			Vec3f s_pos= Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) - Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
			
			s_pos=s_pos*(k_shear);
			
			Vec3f pos2=p2.getPosition();
			//glVertex3f(pos.x(),pos.y(),pos.z());
			//glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());
			//glVertex3f(pos2.x(),pos2.y(),pos2.z());
			//glVertex3f(shear_force.x(),shear_force.y(),shear_force.z());
			
		}
		for(int k=0; k<p.bend.size(); k++)
		{
			const ClothParticle &p2=getParticle(p.bend[k].first,p.bend[k].second);
			//calculate bend spring force on p from p2
			Vec3f s_pos= (p2.getPosition()-p.getPosition()) - (p2.getOriginalPosition()-p.getOriginalPosition());   
			s_pos=s_pos*(k_bend);
			bend_force=s_pos;//bend_force+s_pos;
			
			Vec3f pos2=p2.getPosition();
			//glVertex3f(pos.x(),pos.y(),pos.z());
			//glVertex3f(pos.x()+s_pos.x(),pos.y()+s_pos.y(),pos.z()+s_pos.z());
			//glVertex3f(pos2.x(),pos2.y(),pos2.z());
			//glVertex3f(bend_force.x(),bend_force.y(),bend_force.z());
			
		}
		
		//glVertex3f(pos.x(),pos.y(),pos.z());
		//glVertex3f(struc_force.x()+2,struc_force.y()+2,struc_force.z()+2);
		
		//glVertex3f(pos.x(),pos.y(),pos.z());
		//glVertex3f(shear_force.x()/1000,shear_force.y()/1000,shear_force.z()/1000);
		
		//glVertex3f(pos.x(),pos.y(),pos.z());
		//glVertex3f(bend_force.x()/1000,bend_force.y()/1000,bend_force.z()/1000);
		
		Vec3f gravity_force=args->gravity*p.getMass();
		//glVertex3f(pos.x(),pos.y(),pos.z());      
		//glVertex3f(pos.x(),pos.y()+gravity_force.y(),pos.z());
		
		
		//const Vec3f &pos = p.getPosition();
		//const Vec3f &force = p.getAcceleration()*10;//Force();
		//glVertex3f(pos.x(),pos.y(),pos.z());
		//glVertex3f(force.x(),force.y(),force.z());
        
		// ASSIGNMENT:
		// visualize the forces acting on each particle
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  } 

  // =====================================================================================
  // render the cloth surface
  // =====================================================================================

  if (args->surface) {
    glColor3f(1,1,1);
    glBegin(GL_QUADS);
    for (int i = 0; i < nx-1; i++) {
      for (int j = 0; j < ny-1; j++) {
	const Vec3f &a = getParticle(i,j).getPosition();
	const Vec3f &b = getParticle(i,j+1).getPosition();
	const Vec3f &c = getParticle(i+1,j+1).getPosition();
	const Vec3f &d = getParticle(i+1,j).getPosition();
	insertNormal(a,b,c,d);
	glVertex3f(a.x(),a.y(),a.z());
	glVertex3f(b.x(),b.y(),b.z());
	glVertex3f(c.x(),c.y(),c.z());
	glVertex3f(d.x(),d.y(),d.z());
      }
    }
    glEnd();
  }

  // =====================================================================================
  // render the wireframe cloth (the structural and shear springs)
  // =====================================================================================

  if (args->wireframe) {
    glDisable(GL_LIGHTING);
    glLineWidth(1);
    glBegin(GL_LINES);
    for (int i = 0; i < nx-1; i++) {
      for (int j = 0; j < ny-1; j++) {
        const Vec3f &a_o = getParticle(i,j).getOriginalPosition();
        const Vec3f &b_o = getParticle(i,j+1).getOriginalPosition();
        const Vec3f &c_o = getParticle(i+1,j+1).getOriginalPosition();
        const Vec3f &d_o = getParticle(i+1,j).getOriginalPosition();        
        const Vec3f &a = getParticle(i,j).getPosition();
        const Vec3f &b = getParticle(i,j+1).getPosition();
        const Vec3f &c = getParticle(i+1,j+1).getPosition();
        const Vec3f &d = getParticle(i+1,j).getPosition();        
        DrawSpring(a_o,b_o,a,b, provot_structural_correction);
        DrawSpring(a_o,c_o,a,c, provot_shear_correction);
        DrawSpring(a_o,d_o,a,d, provot_structural_correction);
        DrawSpring(b_o,c_o,b,c, provot_structural_correction);
        DrawSpring(b_o,d_o,b,d, provot_shear_correction);
        DrawSpring(c_o,d_o,c,d, provot_structural_correction);
      }
    }
    glEnd();
    glEnable(GL_LIGHTING);
  }
}

// ================================================================================
// ================================================================================

void Cloth::Animate() 
{
	for (int i = 0; i < nx; i++) 
	{
		for (int j = 0; j < ny; j++) 
		{
			ClothParticle &p = getParticle(i,j);
			p.setAcceleration(Vec3f(0,0,0));
			Vec3f struc_force=Vec3f(0,0,0);
			Vec3f shear_force=Vec3f(0,0,0);
			Vec3f bend_force=Vec3f(0,0,0);
			Vec3f check_f=Vec3f(0,0,0);
			Vec3f check_f_shear=Vec3f(0,0,0);
			//Vec3f check_f2=Vec3f(0,0,0);
			pair<int,int> f2,f2_shear;
			
			bool can_move=true;
			bool can_move2=true;
			bool can_move3=true;
			bool can_move4=true;
			for(int k=0; k<p.struc.size(); k++)
			{
				const ClothParticle &p2=getParticle(p.struc[k].first,p.struc[k].second);
				Vec3f a=p2.getPosition();
				Vec3f b=p.getPosition();
				Vec3f c=p2.getOriginalPosition();
				Vec3f d=p.getOriginalPosition();
				//calculate struct spring force on p from p2
				//Vec3f s_pos= (p2.getPosition()-p.getPosition()) - (p2.getOriginalPosition()-p.getOriginalPosition());  
				Vec3f s_pos= Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) - Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );
				
				s_pos=s_pos*(k_structural);
				struc_force=struc_force+s_pos;
				
				float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
				float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
				float check=dist/dist2;    //distance(p2.getOriginalPosition(),p.getOriginalPosition());
				
				//cout<<check<<" "<<provot_structural_correction<<endl;
				if( (check > (provot_structural_correction+1)) && p2.isFixed()==true )
				{
					can_move=false;
					check_f=p2.getPosition();
				}
				else if( (check > (provot_structural_correction+1)) )
				{
					can_move2=false;
					f2=make_pair(p.struc[k].first,p.struc[k].second); //f2=p2;//check_f2=p2.getPosition();
				}	
				if(s_pos==Vec3f(0,0,0))
					p.setOriginalPosition(p.getPosition());
				//else if( p.getVelocity()==Vec3f(0,0,0) )
				//{
				//	p.setOriginalPosition(p.getPosition());
				//}
				
			}
			for(int k=0; k<p.shear.size(); k++)
			{
				const ClothParticle &p2=getParticle(p.shear[k].first,p.shear[k].second);
				Vec3f a=p2.getPosition();
				Vec3f b=p.getPosition();
				Vec3f c=p2.getOriginalPosition();
				Vec3f d=p.getOriginalPosition();
				//calculate shear spring force on p from p2
				//Vec3f s_pos= (p2.getPosition()-p.getPosition()) - (p2.getOriginalPosition()-p.getOriginalPosition());  
				Vec3f s_pos= Vec3f( a.x()-b.x(), a.y()-b.y(), a.z()-b.z() ) - Vec3f( c.x()-d.x(), c.y()-d.y(), c.z()-d.z() );				
				
				s_pos=s_pos*(k_shear);
				shear_force=shear_force+s_pos;
				
				float dist= sqrt( (a.x()-b.x())*(a.x()-b.x()) + (a.y()-b.y())*(a.y()-b.y()) + (a.z()-b.z())*(a.z()-b.z()) );
				float dist2=sqrt( (c.x()-d.x())*(c.x()-d.x()) + (c.y()-d.y())*(c.y()-d.y()) + (c.z()-d.z())*(c.z()-d.z()) );
				float check=dist/dist2;    //distance(p2.getOriginalPosition(),p.getOriginalPosition());
				
				//cout<<check<<" "<<provot_structural_correction<<endl;
				if( (check > (provot_shear_correction+1)) && p2.isFixed()==true )
				{
					can_move3=false;
					check_f_shear=p2.getPosition();
				}
				else if( (check > (provot_shear_correction+1)) )
				{
					can_move4=false;
					f2_shear=make_pair(p.struc[k].first,p.struc[k].second); //f2=p2;//check_f2=p2.getPosition();
				}	
				if(s_pos==Vec3f(0,0,0))
					p.setOriginalPosition(p.getPosition());
			}
			for(int k=0; k<p.bend.size(); k++)
			{
				const ClothParticle &p2=getParticle(p.bend[k].first,p.bend[k].second);
				//calculate bend spring force on p from p2
				Vec3f s_pos= (p2.getPosition()-p.getPosition()) - (p2.getOriginalPosition()-p.getOriginalPosition());   
				s_pos=s_pos*(k_bend);
				bend_force=bend_force+s_pos;
			}
			if(p.isFixed()==false)
			{
				Vec3f gravity_force=args->gravity*p.getMass();
				Vec3f accel = struc_force+shear_force+bend_force+gravity_force;
				
				if(p.getOriginalPosition().x()==.25 && p.getOriginalPosition().y()==.5)
				{
					cout<<p.getPosition()<<endl;
					cout<<" "<<struc_force<<endl;
					cout<<" "<<shear_force<<endl;
					cout<<" "<<bend_force<<endl;
					cout<<" "<<gravity_force<<endl;
					cout<<endl;
					cout<<"  "<<accel<<endl;
					cout<<"--------------"<<endl;
				}
				
				accel=Vec3f(accel.x()/p.getMass(),accel.y()/p.getMass(),accel.z()/p.getMass());
				p.setAcceleration(accel*args->timestep);//p.getAcceleration()+accel*args->timestep);
				
				//p.setAcceleration(p.getAcceleration()+(Vec3f(0,-9.81,0)*args->timestep) );
				Vec3f Vi=p.getVelocity();
				Vec3f Vf=p.getVelocity()+( (p.getAcceleration()) * args->timestep);
				Vec3f Vif=Vi+Vf;
				
				if(can_move==true && can_move2==true && can_move3==true && can_move4==true)
				{
					p.setPosition( p.getPosition() + ( ( Vec3f(Vif.x()/2,Vif.y()/2,Vif.z()/2) )*args->timestep) );  //p.setPosition(p.getPosition()+(p.getVelocity()*args->timestep));
					//p.setOriginalPosition(p.getPosition());
				}
				if(can_move==false)
				{
					Vec3f pos=p.getPosition();
					Vec3f pos2=check_f;
					Vec3f midpoint=(Vec3f( (pos.x()+pos2.x())/2, (pos.y()+pos2.y())/2, (pos.z()+pos2.z())/2 ) );
					Vec3f midupper=Vec3f( (pos.x()+midpoint.x())/2, (pos.y()+midpoint.y())/2, (pos.z()+midpoint.z())/2 ) ;
					Vec3f midupper2=Vec3f( (pos.x()+midupper.x())/2, (pos.y()+midupper.y())/2, (pos.z()+midupper.z())/2 ) ;
					p.setPosition(midupper2);
					
					p.setVelocity(Vec3f(0,0,0));
					
				}
				if(can_move2==false)
				{
					//p.setPosition( p.getPosition() + ( ( Vec3f(Vif.x()/2,Vif.y()/2,Vif.z()/2) )*args->timestep) );
					Vec3f pos=p.getPosition();
					ClothParticle &p2=getParticle(f2.first,f2.second);
					Vec3f pos2=p2.getPosition();//check_f2;
					Vec3f midpoint=Vec3f( (pos.x()+pos2.x())/2, (pos.y()+pos2.y())/2, (pos.z()+pos2.z())/2 ) ;
					Vec3f midupper=Vec3f( (pos.x()+midpoint.x())/2, (pos.y()+midpoint.y())/2, (pos.z()+midpoint.z())/2 ) ;
					Vec3f midlower=Vec3f( (midpoint.x()+pos2.x())/2, (midpoint.y()+pos2.y())/2, (midpoint.z()+pos2.z())/2 ) ;
					
					Vec3f midupper2=Vec3f( (pos.x()+midupper.x())/2, (pos.y()+midupper.y())/2, (pos.z()+midupper.z())/2 ) ;
					Vec3f midlower2=Vec3f( (midlower.x()+pos2.x())/2, (midlower.y()+pos2.y())/2, (midlower.z()+pos2.z())/2 ) ;
					
					Vec3f midupper3=Vec3f( (pos.x()+midupper2.x())/2, (pos.y()+midupper2.y())/2, (pos.z()+midupper2.z())/2 ) ;
					Vec3f midlower3=Vec3f( (midlower2.x()+pos2.x())/2, (midlower2.y()+pos2.y())/2, (midlower2.z()+pos2.z())/2 ) ;
					
					Vec3f midupper4=Vec3f( (pos.x()+midupper3.x())/2, (pos.y()+midupper3.y())/2, (pos.z()+midupper3.z())/2 ) ;
					Vec3f midlower4=Vec3f( (midlower3.x()+pos2.x())/2, (midlower3.y()+pos2.y())/2, (midlower3.z()+pos2.z())/2 ) ;
					
					Vec3f midupper5=Vec3f( (pos.x()+midupper4.x())/2, (pos.y()+midupper4.y())/2, (pos.z()+midupper4.z())/2 ) ;
					Vec3f midlower5=Vec3f( (midlower4.x()+pos2.x())/2, (midlower4.y()+pos2.y())/2, (midlower4.z()+pos2.z())/2 ) ;
					
					p.setPosition(midupper5);
					p2.setPosition(midlower5);
					//p.setVelocity(Vec3f(0,0,0));
					//p2.setVelocity(Vec3f(0,0,0));
				}
				if(can_move3==false)
				{
					Vec3f pos=p.getPosition();
					Vec3f pos2=check_f_shear;
					Vec3f midpoint=(Vec3f( (pos.x()+pos2.x())/2, (pos.y()+pos2.y())/2, (pos.z()+pos2.z())/2 ) );
					Vec3f midupper=Vec3f( (pos.x()+midpoint.x())/2, (pos.y()+midpoint.y())/2, (pos.z()+midpoint.z())/2 ) ;
					Vec3f midupper2=Vec3f( (pos.x()+midupper.x())/2, (pos.y()+midupper.y())/2, (pos.z()+midupper.z())/2 ) ;
					p.setPosition(midupper2);
					
					p.setVelocity(Vec3f(0,0,0));
				}
				if(can_move4==false)
				{
					//p.setPosition( p.getPosition() + ( ( Vec3f(Vif.x()/2,Vif.y()/2,Vif.z()/2) )*args->timestep) );
					Vec3f pos=p.getPosition();
					ClothParticle &p2=getParticle(f2_shear.first,f2_shear.second);
					Vec3f pos2=p2.getPosition();//check_f2;
					
					Vec3f midpoint=Vec3f( (pos.x()+pos2.x())/2, (pos.y()+pos2.y())/2, (pos.z()+pos2.z())/2 ) ;
					Vec3f midupper=Vec3f( (pos.x()+midpoint.x())/2, (pos.y()+midpoint.y())/2, (pos.z()+midpoint.z())/2 ) ;
					Vec3f midlower=Vec3f( (midpoint.x()+pos2.x())/2, (midpoint.y()+pos2.y())/2, (midpoint.z()+pos2.z())/2 ) ;
					Vec3f midupper2=Vec3f( (pos.x()+midupper.x())/2, (pos.y()+midupper.y())/2, (pos.z()+midupper.z())/2 ) ;
					Vec3f midlower2=Vec3f( (midlower.x()+pos2.x())/2, (midlower.y()+pos2.y())/2, (midlower.z()+pos2.z())/2 ) ;
					Vec3f midupper3=Vec3f( (pos.x()+midupper2.x())/2, (pos.y()+midupper2.y())/2, (pos.z()+midupper2.z())/2 ) ;
					Vec3f midlower3=Vec3f( (midlower2.x()+pos2.x())/2, (midlower2.y()+pos2.y())/2, (midlower2.z()+pos2.z())/2 ) ;
					Vec3f midupper4=Vec3f( (pos.x()+midupper3.x())/2, (pos.y()+midupper3.y())/2, (pos.z()+midupper3.z())/2 ) ;
					Vec3f midlower4=Vec3f( (midlower3.x()+pos2.x())/2, (midlower3.y()+pos2.y())/2, (midlower3.z()+pos2.z())/2 ) ;
					Vec3f midupper5=Vec3f( (pos.x()+midupper4.x())/2, (pos.y()+midupper4.y())/2, (pos.z()+midupper4.z())/2 ) ;
					Vec3f midlower5=Vec3f( (midlower4.x()+pos2.x())/2, (midlower4.y()+pos2.y())/2, (midlower4.z()+pos2.z())/2 ) ;
					
					p.setPosition(midupper5);
					p2.setPosition(midlower5);
					//p.setVelocity(Vec3f(0,0,0));
					//p2.setVelocity(Vec3f(0,0,0));
				}
				p.setVelocity(Vf);
			}
			else
				p.setPosition(p.getOriginalPosition());
			
			//const Vec3f &b = getParticle(i,j+1).getPosition();
			//const Vec3f &c = getParticle(i+1,j+1).getPosition();
			//const Vec3f &d = getParticle(i+1,j).getPosition();
		}
	}
  // ASSIGNMENT:
  // move the vertices of the mesh
	
	
}


// ================================================================================
// ================================================================================
// some helper drawing functions
// ================================================================================

void DrawSpring(const Vec3f &a_o, const Vec3f &b_o, const Vec3f &a, const Vec3f &b, float correction) {
  Vec3f ab_o = b_o-a_o;
  Vec3f ab = b-a;
  float length_o = ab_o.Length(); // the original length
  float length = ab.Length();     // the current length
  if (length > (1+correction) * length_o ||
      length < (1-correction) * length_o) {
    // draw the spring in cyan if it's over-stretched
    glColor3f(0,1,1);
  } else {
    // otherwise draw it black
    glColor3f(0,0,0);
  }  
  glVertex3f(a.x(),a.y(),a.z());
  glVertex3f(b.x(),b.y(),b.z());
}

void insertNormal(const Vec3f &p1, const Vec3f &p2, const Vec3f &p3, const Vec3f &p4) {
  // compute the normal of a non-planar quadrilateral (not well-defined)
  Vec3f v12 = p2 - p1;
  Vec3f v23 = p3 - p2;
  Vec3f v34 = p4 - p3;
  Vec3f v41 = p1 - p4;

  // compute normals at each corner and average them
  Vec3f normal1, normal2, normal3, normal4;
  Vec3f::Cross3(normal1,v41,v12);
  Vec3f::Cross3(normal2,v12,v23);
  Vec3f::Cross3(normal3,v23,v34);
  Vec3f::Cross3(normal4,v34,v41);
  Vec3f normal = normal1+normal2+normal3+normal4;

  normal.Normalize();
  glNormal3f(normal.x(), normal.y(), normal.z());
}

void DrawForce(const Vec3f &p, const Vec3f &f) {
  Vec3f tmp = p+f;
  glVertex3f(p.x(),p.y(),p.z());
  glVertex3f(tmp.x(),tmp.y(),tmp.z());
}

// ================================================================================
// ================================================================================

